#2008/11/12 Added the Range scanner 

require 'socket'


#Add the control modules
require "gui_control_lib/level1/main/modules/control_module.rb"
include Control_module

#Add the sensor controls
require "gui_control_lib/level1/main/modules/sensordata_module.rb"
include Sensordata_module

#Add the collision avoidance modules
require "gui_control_lib/level1/main/modules/colad_module.rb"
include Colad_module

#Class Robot creates one simulated Robot in the world of USARSIM 
# Use the init method to connect to the USARSIM server
# The actual robot will not be reflected in the game world unless you use the Robot.init method
 class Robot
  
  
  # The Constructors are the constructors needed to connect to the server
  # and generate a new robot
  
  #ex. initialize("10.1.25.5",3000) connects to an USARSIM server at ip 10.1.25.5 at
  #port 3000
  #  Note:Default port for USARSIM is 3000 
  def initialize(ip,port)
    
    @ip=ip
    @port=port
    @m_out=""
   
    #Robotic information arrays
    @sen= Array.new  # sen=[] (Robotic Sensor)
    @status=[] # status[0]=time, status[1]=battery
    @ins=[] # INS Information {X,Y,Z} 
    @fSONARSEN=[]# Front Sonar sensors
    @rSONARSEN=[]# Rear Sonar sensors
    @moveflag="" #Check if robot is moving
    @rangescan=[] # Range sensors
    
    #Class Variables 
    @connect  #connection to server, called by init function
    @robottype=""
    @startpos="" 
    @robotname=""
    
    #used movements
    @speed=2.0 #the rad/s the robot is moving
    
    
    #for collision checks
    @static_col=0
    #@static_col_back=0
    
    #SDK only (used as a flag when looping the display and recieve functions)
    @displayflag=0
    @recieveflag=0
    
    #ensure that recieve has finished running
    @rflag=0
    
    #Constant Parameters 
    @P2ATPARA=0.125 # The parameter for controlling movement in the smart
    #move functions, 0.125 is accuarate for the motor speeds Left 1.0, Right 1.0
    
    #cheat
    @truepos=[]
    
  end
    
  # sensor information of the robot, returned as an array of strings
  attr_reader :sen
  # status information of the robot, returned as an array of strings
  attr_reader :status
  # INS sensor infomation, returned as an array of strings
  attr_reader :ins
  # Front Sonar sensor infomation, returned as an array of strings
  attr_reader :fSONARSEN
  # Rear Sonar sensor infomation, returned as an array of strings
  attr_reader :rSONARSEN
  # RangeScanner information, returned as an array of strings
  attr_reader :rangescan
  # The robot's true position
  attr_reader :truepos
  # The victim sensor
  attr_reader:victsen
  attr_reader:templine
  
  #Special static_collision indication, if=1 then has crashed
  attr_reader :static_col
  #used to make mapping more efficient
  attr_reader :moveflag
  #the robot's name, used with the log function
  attr_reader :robotname
  

  
 #creates a robot in the gameworld, inputs the robot type and the starting position and the robot
 #as a string argument. if the starting position is NULL the program will use a default
 #starting position for the map. if the 
 # Ex. @robot.init("USARBot.P2AT","","")  #generates a P2AT robot at the position "0.76,2.3,1.8"
 def init(robottype,startpos,robotname)
   @robottype=robottype
   @startpos=startpos
   @robotname=robotname
   
   if @startpos==""
     #default starting value for factory map
    @startpos="0.76,2.3,1.8"
   end
   #Set Try to connect twice
   trys=0
   
   begin
   puts "Connecting to USARSIM server"
   @connect =TCPsocket.open(@ip, @port)
   rescue Exception
    trys=trys+1
    sleep(5) unless trys>=2
    retry unless trys>=2
    raise "Cannot connect to USARSIM server"
   end
   
   @connect.write("INIT {ClassName "<<@robottype<<"} {Location "<< @startpos <<"}\r\n") if @robotname==""
   @connect.write("INIT {ClassName "<<@robottype<<"} {Name "<< @robotname <<"} {Location "<< @startpos <<"}\r\n") if @robotname!=""
      
   #wait till recieve a response from the server (display lib)
   puts "Robot connected waiting for server"
   while(@sen[0]==nil)
     self.recieve
     sleep(1)
   end
   puts "wait complete"
   
   
 end


 #the robot continues to display until the program stops
 def displayloop
   @displayflag=1
   Thread.new{
   Thread.current["name"]="r_#{@robotname}_displayloop"
   while(@displayflag==1)
     self.display
     sleep(0.5)
   end
   }
 end
 
 
 #the robot continues to communicate with the server
 def recievestart
   Thread.new{
   Thread.current["name"]="r_#{@robotname}_recieveloop"
   @recieveflag=1
   while(@recieveflag==1)
     self.recieve

     sleep(0.1)
   end
   }
 end
 
 #the robot stops communicating with the server
 def recievestop
   @recieveflag=0 if @recieveflag==1
 end
 
 #stops the robot from displaying 
 def displaystop
   @display=0 if @displayflag==1
 end
   
 
 
  #display method outputs the robot's sensor information and status information
  #in a console window
  def display 
   
    system("cls")
 
    # Display the normal Status
    puts "GLOBAL TIME=#{@status[0]}  Battery=#{@status[1]} "  
   
    # Display the sensor Status
    puts "Position X=#{@ins[0]}"
    puts "Position Y=#{@ins[1]}"
    puts "Position Z=#{@ins[2]}"
    puts "ROLL=#{@ins[3]}"
    puts "PITCH=#{@ins[4]}"
    puts "YAW=#{@ins[5]}" 
    
   #draw sensors
   #sonarsensors
    i=0  
     while i<8
       puts "SonarF#{i+1} :#{@fSONARSEN[i]}" unless @fSONARSEN[i]==nil
       i=i+1
     end
    i=0
    while i<8
      puts "SonarR#{i+1} :#{@rSONARSEN[i]}" unless @rSONARSEN[i]==nil
       i=i+1
     end   
    
   
  end
  
  #the recieve method communicates with the USARSIM server and 
  #scans the return message from the server for sensor and status information
  #it also produces an array which contains the sensor infomation
  def recieve
    
    if(@rflag==0)
    @rflag=1
    revdata=@connect.recv(3000)
    revdata.each{|line| 
     ############ input the cheat value #########################
     if line =~/Type GroundTruth/i and line[line.length-1]==10 
       temparray=line.split("{")
       temparray.each{|mem|
         if mem=~/Location/i
           temparray2=mem.split(" ")
           temparray2=temparray2[1].split(",")
           @truepos[0]=temparray2[0].to_f
           @truepos[1]=temparray2[1].to_f
           @truepos[2]=(temparray2[2][0..temparray2[2].length-1]).to_f
         end
         
         if mem=~/Orientation/i
           temparray2=mem.split(" ")
           temparray2=temparray2[1].split(",")
           @truepos[3]=temparray2[0].to_f
           @truepos[4]=temparray2[1].to_f
           @truepos[5]=(temparray2[2][0..temparray2[2].length-1]).to_f
         end
       }
       #puts @truepos
     end   
        
     ########## input the Victim Sensor  #######################################    
     #SEN {Time 104.9735} {Type VictSensor} {Status NoVictims}#
     #SEN {Time 716.7440} {Type VictSensor} {Name VictSensor} {PartName Hand} {Location 2.08,-0.28,0.12} {PartName Arm} {Location 2.38,-0.51,0.03} {PartName Chest} {Location 2.53,-0.31,0.19} {PartName Arm} {Location 2.45,-0.45,0.50} {PartName Hand} {Location 2.50,-0.66,-0.18} {PartName Head} {Location 2.23,-0.38,0.20} {PartName Leg} {Location 3.04,-0.33,0.36} {PartName Head} {Location 1.96,-0.08,0.20}
     if line=~/Type VictSensor/i and line[line.length-1]==10
       #puts line
       @victsen=""
       temparray=line.split("{")
       temparray.each{|mem|
       if mem=~/NoVictims/i #no victims
           temparray2=mem.split(" ")
           temparray2=temparray2[1].to_s
           temparray2=temparray2[0..temparray2.length-2]
           @victsen=temparray2
           break
       end        
       }
       if(@victsen=="") #found victim
       #puts "Red alert"
       @victsen=Array.new()
       insertarray=""
        temparray.each{|mem|
          if mem=~/PartName/i
            temparray2=mem.split(" ")
            temparray2=temparray2[1].to_s
            temparray2=temparray2[0..temparray2.length-2]
            insertarray=temparray2
          end
          if mem=~/Location/i
            temparray2=mem.split(" ")
            temparray2=temparray2[1].to_s
            temparray2=temparray2[0..temparray2.length-2]
            insertarray=temparray2
          end
        @victsen<<insertarray if insertarray!=""
        }
       end
     end
        
     ############### inputing the status ARRAY #################################### 
     if line[0,3]=="STA" and line[line.length-1]==10 
      temparray=line.split(" {")
      temparray.each { |mem| 
      if mem[0,4]=="Time"
        @status[0]=mem[5..mem.index('}')-1]  #if error change to [5,mem.index('}')-1]
        
      end
      if mem[0,4]=="Batt"
        @status[1]=mem[7..mem.index('}')-1]
      end
      }  
     end
     
 
     ############## inputing sensor data is SEN ARRAY############################### 
     if line[0,3]=="SEN" and line[line.length-1]==10   #get only completed data lines(10= New line)
     #check type in array 
     inarray=FALSE
      temparray=line.split("{Type ") 
      sentype=temparray[1]
      sentype=sentype[0,sentype.index('}')]
        @sen.each_with_index {|arraymember,index|
        changeflag=false
        arraymember.each{|member|
         if member.index('Type')!=nil
            if sentype==member[5..member.length] 
              inarray=TRUE
              changeflag=TRUE
              #if this type of sensor has already been recorded then true
            end  
         end
        }
        if changeflag==TRUE
        temparray=line.split(" {")
        temparray2=Array.new
        temparray.each { |mem|
        if mem!='SEN' # if not the SEN Indentifier array
           temparray2 << mem[0,mem.index("}")]
        end
        }
        @sen[index]=temparray2
       end

      }      
      if inarray==FALSE #not in any array
        @sen=@sen << Array.new # add new array member to the sensor array [[],[],..[*added]]
     
        temparray=line.split(" {")
        temparray.each { |mem|
        if mem!='SEN' # if not the SEN Indentifier array
          @sen[@sen.length-1][@sen[@sen.length-1].length]=mem[0,mem.index("}")] if mem.index("}")!=nil
          # add a member to the last array-member of the sensor array
          # @sen[@sen.length-1] is the last member of the sensor array 
          # @sen[@sen.length-1].length is the last length of the last array
        end
        }
      end
      
    end
    }
  ############## end of inputing sensor data is SEN ARRAY############################### 
     
   #encode the sensor infomation into The INS Arrays and sonar Arrays

   
   #at first used,but slow
   @ins=self.getinsinfo(@sen)
   @fSONARSEN=self.getfsonarsensor(@sen)
   @rSONARSEN=self.getrsonarsensor(@sen)
   @rangescan=self.getrangescanner(@sen)
   
    @rflag=0  
    end #recieve flag

  end # end def
  

 
    
end
